/*
 * Copyright (c) 2011-2014, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.geo;

import boofcv.abst.geo.Estimate1ofEpipolar;
import boofcv.abst.geo.f.EstimateNto1ofEpipolar;
import boofcv.alg.geo.f.DistanceEpipolarConstraint;
import boofcv.factory.geo.EnumEpipolar;
import boofcv.factory.geo.FactoryMultiView;
import boofcv.struct.geo.AssociatedPair;
import boofcv.struct.geo.GeoModelEstimator1;
import boofcv.struct.geo.GeoModelEstimatorN;
import georegression.geometry.GeometryMath_F64;
import georegression.geometry.RotationMatrixGenerator;
import georegression.struct.point.Point3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.transform.se.SePointOps_F64;
import org.ejml.data.DenseMatrix64F;
import org.ejml.ops.CommonOps;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.Random;

/**
 * @author Peter Abeles
 */
public class BenchmarkStabilityFundamental {

	Random rand = new Random(265);

	double sigmaPixels = 0;

	int totalPoints = 100;
	double sceneCenterZ = 3;
	double sceneRadius = 2;

	List<Point3D_F64> scene;
	List<AssociatedPair> observations;

	// create a reasonable calibration matrix
	DenseMatrix64F K = new DenseMatrix64F(3,3,true,60,0.01,-200,0,80,-150,0,0,1);
	DenseMatrix64F K_inv = new DenseMatrix64F(3,3);
	// relationship between both camera
	protected Se3_F64 motion;

	// are observations in pixels or normalized image coordinates
	public static boolean isPixels = false;

	List<Double> scores;

	public BenchmarkStabilityFundamental() {
		CommonOps.invert(K, K_inv);
	}

	public void createSceneCube() {
		scene = new ArrayList<Point3D_F64>();

		for( int i = 0; i < totalPoints; i++ ) {
			Point3D_F64 p = new Point3D_F64();

			p.x = (rand.nextDouble()-0.5)*2*sceneRadius;
			p.y = (rand.nextDouble()-0.5)*2*sceneRadius;
			p.z = (rand.nextDouble()-0.5)*2*sceneRadius+sceneCenterZ;

			scene.add(p);
		}
	}

	public void createScenePlane() {
		scene = new ArrayList<Point3D_F64>();

		for( int i = 0; i < totalPoints; i++ ) {
			Point3D_F64 p = new Point3D_F64();

			p.x = (rand.nextDouble()-0.5)*2*sceneRadius;
			p.y = (rand.nextDouble()-0.5)*2*sceneRadius;
			p.z = sceneCenterZ;

			scene.add(p);
		}
	}

	public void motionTranslate() {
		motion = new Se3_F64();
		motion.getT().set(0.2,0,0);
	}

	public void motionTransRot() {
		motion = new Se3_F64();
		motion.getR().set(RotationMatrixGenerator.eulerXYZ(0.01,-0.02,0.05,null));
		motion.getT().set(0.2,0,0);
	}

	public void createObservations() {
		observations = new ArrayList<AssociatedPair>();

		for( Point3D_F64 p1 : scene ) {
			Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);

			if( p1.z < 0 || p2.z < 0 )
				continue;

			AssociatedPair pair = new AssociatedPair();
			pair.p1.set(p1.x/p1.z,p1.y/p1.z);
			pair.p2.set(p2.x/p2.z,p2.y/p2.z);
			observations.add(pair);

			// convert to pixels
			GeometryMath_F64.mult(K, pair.p1, pair.p1);
			GeometryMath_F64.mult(K, pair.p2,pair.p2);

			// add noise
			pair.p1.x += rand.nextGaussian()*sigmaPixels;
			pair.p1.y += rand.nextGaussian()*sigmaPixels;
			pair.p2.x += rand.nextGaussian()*sigmaPixels;
			pair.p2.y += rand.nextGaussian()*sigmaPixels;

			// if needed, convert back into normalized image coordinates
			if( !isPixels ) {
				GeometryMath_F64.mult(K_inv, pair.p1, pair.p1);
				GeometryMath_F64.mult(K_inv, pair.p2,pair.p2);
			}
		}
	}

	public void evaluateMinimal( GeoModelEstimatorN<DenseMatrix64F,AssociatedPair> estimatorN ) {

		DistanceEpipolarConstraint distance = new DistanceEpipolarConstraint();

		Estimate1ofEpipolar estimator =
				new EstimateNto1ofEpipolar(estimatorN,distance,1);

		scores = new ArrayList<Double>();
		int failed = 0;

		int numSamples = estimator.getMinimumPoints();

		Random rand = new Random(234);

		DenseMatrix64F F = new DenseMatrix64F(3,3);

		for( int i = 0; i < 50; i++ ) {
			List<AssociatedPair> pairs = new ArrayList<AssociatedPair>();

			// create a unique set of pairs
			while( pairs.size() < numSamples) {
				AssociatedPair p = observations.get( rand.nextInt(observations.size()) );

				if( !pairs.contains(p) ) {
					pairs.add(p);
				}
			}

			if( !estimator.process(pairs,F) ) {
				failed++;
				continue;
			}

			// normalize the scale of F
			CommonOps.scale(1.0/CommonOps.elementMaxAbs(F),F);

			double totalScore = 0;
			// score against all observations
			for( AssociatedPair p : observations ) {
				double score = Math.abs(GeometryMath_F64.innerProd(p.p2, F, p.p1));
				if( Double.isNaN(score))
					System.out.println("Score is NaN");
				scores.add(score);
				totalScore += score;
			}
//			System.out.println("  score["+i+"] = "+totalScore);
		}

		Collections.sort(scores);

		System.out.printf(" Failures %3d  Score:  50%% = %6.3e  95%% = %6.3e\n", failed, scores.get(scores.size() / 2), scores.get((int) (scores.size() * 0.95)));
	}

	public void evaluateAll( GeoModelEstimator1<DenseMatrix64F,AssociatedPair> estimator ) {
		scores = new ArrayList<Double>();
		int failed = 0;

		DenseMatrix64F F = new DenseMatrix64F(3,3);

		for( int i = 0; i < 50; i++ ) {

			if( !estimator.process(observations,F) ) {
				failed++;
				continue;
			}

			// normalize the scale of F
			CommonOps.scale(1.0/CommonOps.elementMaxAbs(F),F);

			// score against all observations
			for( AssociatedPair p : observations ) {
				double score = Math.abs(GeometryMath_F64.innerProd(p.p2, F, p.p1));
				if( Double.isNaN(score))
					System.out.println("Score is NaN");
				scores.add(score);
			}
		}

		Collections.sort(scores);

		System.out.printf(" Failures %3d  Score:  50%% = %6.3e  95%% = %6.3e\n", failed, scores.get(scores.size() / 2), scores.get((int) (scores.size() * 0.95)));
	}

	public static void main( String args[] ) {
		BenchmarkStabilityFundamental app = new BenchmarkStabilityFundamental();

		app.createSceneCube();
//		app.createScenePlane();
		app.motionTranslate();
		app.createObservations();
		if( isPixels ){
			app.evaluateMinimal(FactoryMultiView.computeFundamental_N(EnumEpipolar.FUNDAMENTAL_8_LINEAR));
			app.evaluateMinimal(FactoryMultiView.computeFundamental_N(EnumEpipolar.FUNDAMENTAL_7_LINEAR));
		} else {
			app.evaluateMinimal(FactoryMultiView.computeFundamental_N(EnumEpipolar.ESSENTIAL_8_LINEAR));
			app.evaluateMinimal(FactoryMultiView.computeFundamental_N(EnumEpipolar.ESSENTIAL_7_LINEAR));
			app.evaluateMinimal(FactoryMultiView.computeFundamental_N(EnumEpipolar.ESSENTIAL_5_NISTER));
		}

//		app.evaluateMinimal(FactoryMultiView.computeFundamental(8));


//		app.evaluateAll(FactoryMultiView.computeEssential(8));
//		app.evaluateAll(FactoryMultiView.computeEssential(5));
	}
}
